using UnityEngine;
using System.Collections;

public class Util
{
    public static bool IsSaneNumber(float f)
    {
        if (System.Single.IsNaN(f)) return false;
        if (f == Mathf.Infinity) return false;
        if (f == Mathf.NegativeInfinity) return false;
        if (f > 1000000000000) return false;
        if (f < -1000000000000) return false;
        return true;
    }

    public static Vector3 Clamp(Vector3 v, float length)
    {
        float l = v.magnitude;
        if (l > length) return v / l * length;
        return v;
    }

    public static float Mod(float x, float period)
    {
        float r = x % period;
        return (r >= 0 ? r : r + period);
    }
    public static int Mod(int x, int period)
    {
        int r = x % period;
        return (r >= 0 ? r : r + period);
    }
    public static float Mod(float x) { return Mod(x, 1); }
    public static int Mod(int x) { return Mod(x, 1); }

    public static float CyclicDiff(float high, float low, float period, bool skipWrap)
    {
        if (!skipWrap)
        {
            high = Mod(high, period);
            low = Mod(low, period);
        }
        return (high >= low ? high - low : high + period - low);
    }
    public static int CyclicDiff(int high, int low, int period, bool skipWrap)
    {
        if (!skipWrap)
        {
            high = Mod(high, period);
            low = Mod(low, period);
        }
        return (high >= low ? high - low : high + period - low);
    }
    public static float CyclicDiff(float high, float low, float period) { return CyclicDiff(high, low, period, false); }
    public static int CyclicDiff(int high, int low, int period) { return CyclicDiff(high, low, period, false); }
    public static float CyclicDiff(float high, float low) { return CyclicDiff(high, low, 1, false); }
    public static int CyclicDiff(int high, int low) { return CyclicDiff(high, low, 1, false); }

    public static bool CyclicIsLower(float compared, float comparedTo, float reference, float period)
    {
        compared = Mod(compared, period);
        comparedTo = Mod(comparedTo, period);
        if (CyclicDiff(compared, reference, period, true) < CyclicDiff(comparedTo, reference, period, true))
            return true;
        return false;
    }
    public static bool CyclicIsLower(int compared, int comparedTo, int reference, int period)
    {
        compared = Mod(compared, period);
        comparedTo = Mod(comparedTo, period);
        if (CyclicDiff(compared, reference, period, true) < CyclicDiff(comparedTo, reference, period, true))
            return true;
        return false;
    }
    public static bool CyclicIsLower(float compared, float comparedTo, float reference)
    {
        return CyclicIsLower(compared, comparedTo, reference, 1);
    }
    public static bool CyclicIsLower(int compared, int comparedTo, int reference)
    {
        return CyclicIsLower(compared, comparedTo, reference, 1);
    }

    public static float CyclicLerp(float a, float b, float t, float period)
    {
        if (Mathf.Abs(b - a) <= period / 2) { return a * (1 - t) + b * t; }
        if (b < a) a -= period; else b -= period;
        return Util.Mod(a * (1 - t) + b * t);
    }

    public static Vector3 ProjectOntoPlane(Vector3 v, Vector3 normal)
    {
        return v - Vector3.Project(v, normal);
    }

    public static Vector3 SetHeight(Vector3 originalVector, Vector3 referenceHeightVector, Vector3 upVector)
    {
        Vector3 originalOnPlane = ProjectOntoPlane(originalVector, upVector);
        Vector3 referenceOnAxis = Vector3.Project(referenceHeightVector, upVector);
        return originalOnPlane + referenceOnAxis;
    }

    public static Vector3 GetHighest(Vector3 a, Vector3 b, Vector3 upVector)
    {
        if (Vector3.Dot(a, upVector) >= Vector3.Dot(b, upVector)) return a;
        return b;
    }
    public static Vector3 GetLowest(Vector3 a, Vector3 b, Vector3 upVector)
    {
        if (Vector3.Dot(a, upVector) <= Vector3.Dot(b, upVector)) return a;
        return b;
    }

    public static Matrix4x4 RelativeMatrix(Transform t, Transform relativeTo)
    {
        return relativeTo.worldToLocalMatrix * t.localToWorldMatrix;
    }

    public static Vector3 TransformVector(Matrix4x4 m, Vector3 v)
    {
        return m.MultiplyPoint(v) - m.MultiplyPoint(Vector3.zero);
    }
    public static Vector3 TransformVector(Transform t, Vector3 v)
    {
        return TransformVector(t.localToWorldMatrix, v);
    }

    public static void TransformFromMatrix(Matrix4x4 matrix, Transform trans)
    {
        trans.rotation = Util.QuaternionFromMatrix(matrix);
        trans.position = matrix.GetColumn(3);
    }

    public static Quaternion QuaternionFromMatrix(Matrix4x4 m)
    {
        Quaternion q = new Quaternion();
        q.w = Mathf.Sqrt(Mathf.Max(0, 1 + m[0, 0] + m[1, 1] + m[2, 2])) / 2;
        q.x = Mathf.Sqrt(Mathf.Max(0, 1 + m[0, 0] - m[1, 1] - m[2, 2])) / 2;
        q.y = Mathf.Sqrt(Mathf.Max(0, 1 - m[0, 0] + m[1, 1] - m[2, 2])) / 2;
        q.z = Mathf.Sqrt(Mathf.Max(0, 1 - m[0, 0] - m[1, 1] + m[2, 2])) / 2;
        q.x *= Mathf.Sign(q.x * (m[2, 1] - m[1, 2]));
        q.y *= Mathf.Sign(q.y * (m[0, 2] - m[2, 0]));
        q.z *= Mathf.Sign(q.z * (m[1, 0] - m[0, 1]));
        return q;
    }

    public static Matrix4x4 MatrixFromQuaternion(Quaternion q)
    {
        return CreateMatrix(q * Vector3.right, q * Vector3.up, q * Vector3.forward, Vector3.zero);
    }

    public static Matrix4x4 MatrixFromQuaternionPosition(Quaternion q, Vector3 p)
    {
        Matrix4x4 m = MatrixFromQuaternion(q);
        m.SetColumn(3, p);
        m[3, 3] = 1;
        return m;
    }

    public static Matrix4x4 MatrixSlerp(Matrix4x4 a, Matrix4x4 b, float t)
    {
        t = Mathf.Clamp01(t);
        Matrix4x4 m = MatrixFromQuaternion(Quaternion.Slerp(QuaternionFromMatrix(a), QuaternionFromMatrix(b), t));
        m.SetColumn(3, a.GetColumn(3) * (1 - t) + b.GetColumn(3) * t);
        m[3, 3] = 1;
        return m;
    }

    public static Matrix4x4 CreateMatrix(Vector3 right, Vector3 up, Vector3 forward, Vector3 position)
    {
        Matrix4x4 m = Matrix4x4.identity;
        m.SetColumn(0, right);
        m.SetColumn(1, up);
        m.SetColumn(2, forward);
        m.SetColumn(3, position);
        m[3, 3] = 1;
        return m;
    }
    public static Matrix4x4 CreateMatrixPosition(Vector3 position)
    {
        Matrix4x4 m = Matrix4x4.identity;
        m.SetColumn(3, position);
        m[3, 3] = 1;
        return m;
    }
    public static void TranslateMatrix(ref Matrix4x4 m, Vector3 position)
    {
        m.SetColumn(3, (Vector3)(m.GetColumn(3)) + position);
        m[3, 3] = 1;
    }

    public static Vector3 ConstantSlerp(Vector3 from, Vector3 to, float angle)
    {
        float value = Mathf.Min(1, angle / Vector3.Angle(from, to));
        return Vector3.Slerp(from, to, value);
    }
    public static Quaternion ConstantSlerp(Quaternion from, Quaternion to, float angle)
    {
        float value = Mathf.Min(1, angle / Quaternion.Angle(from, to));
        return Quaternion.Slerp(from, to, value);
    }
    public static Vector3 ConstantLerp(Vector3 from, Vector3 to, float length)
    {
        return from + Clamp(to - from, length);
    }

    public static Vector3 Bezier(Vector3 a, Vector3 b, Vector3 c, Vector3 d, float t)
    {
        Vector3 ab = Vector3.Lerp(a, b, t);
        Vector3 bc = Vector3.Lerp(b, c, t);
        Vector3 cd = Vector3.Lerp(c, d, t);
        Vector3 abc = Vector3.Lerp(ab, bc, t);
        Vector3 bcd = Vector3.Lerp(bc, cd, t);
        return Vector3.Lerp(abc, bcd, t);
    }

    public static GameObject Create3dText(Font font, string text, Vector3 position, float size, Color color)
    {
        GameObject myTextObject = new GameObject("text_" + text);

        TextMesh textMeshComponent = myTextObject.AddComponent(typeof(TextMesh)) as TextMesh;
        myTextObject.AddComponent(typeof(MeshRenderer));

        textMeshComponent.font = font;
        myTextObject.renderer.material = font.material;
        myTextObject.renderer.material.color = color;

        textMeshComponent.text = text;

        myTextObject.transform.localScale = Vector3.one * size;
        myTextObject.transform.Translate(position);

        return myTextObject;
    }

    public static float[] GetLineSphereIntersections(Vector3 lineStart, Vector3 lineDir, Vector3 sphereCenter, float sphereRadius)
    {
        float a = lineDir.sqrMagnitude;
        float b = 2 * (Vector3.Dot(lineStart, lineDir) - Vector3.Dot(lineDir, sphereCenter));
        float c = lineStart.sqrMagnitude + sphereCenter.sqrMagnitude - 2 * Vector3.Dot(lineStart, sphereCenter) - sphereRadius * sphereRadius;
        float d = b * b - 4 * a * c;
        if (d < 0) return null;
        float i1 = (-b - Mathf.Sqrt(d)) / (2 * a);
        float i2 = (-b + Mathf.Sqrt(d)) / (2 * a);
        if (i1 < i2) return new float[] { i1, i2 };
        else return new float[] { i2, i1 };
    }
}

public class DrawArea
{
    public Vector3 min;
    public Vector3 max;
    public Vector3 canvasMin = new Vector3(0, 0, 0);
    public Vector3 canvasMax = new Vector3(1, 1, 1);

    public DrawArea(Vector3 min, Vector3 max)
    {
        this.min = min;
        this.max = max;
    }

    public virtual Vector3 Point(Vector3 p)
    {
        return Camera.main.ScreenToWorldPoint(Vector3.Scale(new Vector3((p.x - canvasMin.x) / (canvasMax.x - canvasMin.x), (p.y - canvasMin.y)
                                            / (canvasMax.y - canvasMin.y), 0), max - min) + min + Vector3.forward * Camera.main.nearClipPlane * 1.1f);
    }

    public void DrawLine(Vector3 a, Vector3 b, Color c)
    {
        GL.Color(c);
        GL.Vertex(Point(a));
        GL.Vertex(Point(b));
    }

    public void DrawRay(Vector3 start, Vector3 dir, Color c)
    {
        DrawLine(start, start + dir, c);
    }

    public void DrawRect(Vector3 a, Vector3 b, Color c)
    {
        GL.Color(c);
        GL.Vertex(Point(new Vector3(a.x, a.y, 0)));
        GL.Vertex(Point(new Vector3(a.x, b.y, 0)));
        GL.Vertex(Point(new Vector3(b.x, b.y, 0)));
        GL.Vertex(Point(new Vector3(b.x, a.y, 0)));
    }

    public void DrawDiamond(Vector3 a, Vector3 b, Color c)
    {
        GL.Color(c);
        GL.Vertex(Point(new Vector3(a.x, (a.y + b.y) / 2, 0)));
        GL.Vertex(Point(new Vector3((a.x + b.x) / 2, b.y, 0)));
        GL.Vertex(Point(new Vector3(b.x, (a.y + b.y) / 2, 0)));
        GL.Vertex(Point(new Vector3((a.x + b.x) / 2, a.y, 0)));
    }

    public void DrawRect(Vector3 corner, Vector3 dirA, Vector3 dirB, Color c)
    {
        GL.Color(c);
        Vector3[] dirs = new Vector3[] { dirA, dirB };
        for (int i = 0; i < 2; i++)
        {
            for (int dir = 0; dir < 2; dir++)
            {
                Vector3 start = corner + dirs[(dir + 1) % 2] * i;
                GL.Vertex(Point(start));
                GL.Vertex(Point(start + dirs[dir]));
            }
        }
    }

    public void DrawCube(Vector3 corner, Vector3 dirA, Vector3 dirB, Vector3 dirC, Color c)
    {
        GL.Color(c);
        Vector3[] dirs = new Vector3[] { dirA, dirB, dirC };
        for (int i = 0; i < 2; i++)
        {
            for (int j = 0; j < 2; j++)
            {
                for (int dir = 0; dir < 3; dir++)
                {
                    Vector3 start = corner + dirs[(dir + 1) % 3] * i + dirs[(dir + 2) % 3] * j;
                    GL.Vertex(Point(start));
                    GL.Vertex(Point(start + dirs[dir]));
                }
            }
        }
    }
}

public class DrawArea3D : DrawArea
{
    public Matrix4x4 matrix;

    public DrawArea3D(Vector3 min, Vector3 max, Matrix4x4 matrix)
        : base(min, max)
    {
        this.matrix = matrix;
    }

    public override Vector3 Point(Vector3 p)
    {
        return matrix.MultiplyPoint3x4(Vector3.Scale(new Vector3((p.x - canvasMin.x) / (canvasMax.x - canvasMin.x), (p.y - canvasMin.y)
                                        / (canvasMax.y - canvasMin.y), p.z), max - min) + min);
    }
}


public class SmoothFollower
{

    private Vector3 targetPosition;
    private Vector3 position;
    private Vector3 velocity;
    private float smoothingTime;
    private float prediction;

    public SmoothFollower(float smoothingTime)
    {
        targetPosition = Vector3.zero;
        position = Vector3.zero;
        velocity = Vector3.zero;
        this.smoothingTime = smoothingTime;
        prediction = 1;
    }

    public SmoothFollower(float smoothingTime, float prediction)
    {
        targetPosition = Vector3.zero;
        position = Vector3.zero;
        velocity = Vector3.zero;
        this.smoothingTime = smoothingTime;
        this.prediction = prediction;
    }

    public Vector3 Update(Vector3 targetPositionNew, float deltaTime)
    {
        Vector3 targetVelocity = (targetPositionNew - targetPosition) / deltaTime;
        targetPosition = targetPositionNew;

        float d = Mathf.Min(1, deltaTime / smoothingTime);
        velocity = velocity * (1 - d) + (targetPosition + targetVelocity * prediction - position) * d;

        position += velocity * Time.deltaTime;
        return position;
    }

    public Vector3 Update(Vector3 targetPositionNew, float deltaTime, bool reset)
    {
        if (reset)
        {
            targetPosition = targetPositionNew;
            position = targetPositionNew;
            velocity = Vector3.zero;
            return position;
        }
        return Update(targetPositionNew, deltaTime);
    }

    public Vector3 GetPosition() { return position; }
    public Vector3 GetVelocity() { return velocity; }
}